#include <cstdio>
#include <iostream>
#include <limits>
#include "gridmap.h"

/**
 * @brief 绘制地图
 *
 * @return opencv图像
 */
cv::Mat GridMap::renderMap() const
{
	cv::Mat mat(m_height, m_width, CV_8UC3, cvScalar(0, 0, 0));
	float x1, y1, x2, y2;
	float half;

	for (std::vector<Line>::const_iterator line = m_lines.begin();
			line != m_lines.end(); ++line) {
		half = line->width / 2.0f;
		if (line->y1 == line->y2) { // 水平直线
			x1 = line->x1;
			y1 = m_height - line->y1 - half;
			x2 = line->x2;
			y2 = m_height - line->y2 + half;
		} else if (line->x1 == line->x2) { // 垂直直线
			x1 = line->x1 - half;
			y1 = m_height - line->y1;
			x2 = line->x2 + half;
			y2 = m_height - line->y2;
		} else { // 其他
			std::cout << "Line type not supported" << std::endl;
			continue;
		}
		cv::rectangle(mat, cvPoint((int)x1, (int)y1), cvPoint((int)x2, (int)y2), cvScalar(255, 255, 255), CV_FILLED);
	}

	return mat;
}

IplImage * GridMap::renderMapIplImage()
{
	float x1, y1, x2, y2;
	float half;

	if (!m_iplimage) {
		m_iplimage = cvCreateImage(cvSize(m_width, m_height), IPL_DEPTH_8U, 3);
	} else {
		cvSetZero(m_iplimage);
	}

	for (std::vector<Line>::const_iterator line = m_lines.begin();
			line != m_lines.end(); ++line) {
		half = line->width / 2.0f;
		if (line->y1 == line->y2) { // 水平直线
			x1 = line->x1;
			y1 = m_height - line->y1 - half;
			x2 = line->x2;
			y2 = m_height - line->y2 + half;
		} else if (line->x1 == line->x2) { // 垂直直线
			x1 = line->x1 - half;
			y1 = m_height - line->y1;
			x2 = line->x2 + half;
			y2 = m_height - line->y2;
		} else { // 其他
			std::cout << "Line type not supported" << std::endl;
			continue;
		}
		cvRectangle(m_iplimage, cvPoint((int)x1, (int)y1), cvPoint((int)x2, (int)y2), cvScalar(255, 255, 255), CV_FILLED);
	}

	return m_iplimage;
}

/**
 * @brief 解析地图文件
 *
 * @param filename 地图文件
 */
bool GridMap::parseFile(const char *filename)
{
	FILE *file;
	char buf[256];
	bool first_line = true;
	Line line;
	int ret;

	// 打开文件
	file = fopen(filename, "r");
	if (!file) {
		return false;
	}

	// 解析文件
	while (std::fgets(buf, sizeof(buf), file)) {
		if (buf[0] == '#') { // 注释
			continue;
		}

		if (first_line) { // 读取地图大小
			ret = std::sscanf(buf, "%d %d", &m_width, &m_height);
			if (ret != 2 || m_width <= 0 || m_height <= 0) {
				std::cerr << "parse file error: " << buf << std::endl;
				return false;
			}
			first_line = false;
		} else { // 读取直线
			ret = std::sscanf(buf, "(%f, %f) (%f, %f) %f",
					&(line.x1), &(line.y1), &(line.x2), &(line.y2), &(line.width));
			if (ret != 5) {
				std::cerr << "parse file error: " << buf << std::endl;
				return false;
			}
			m_lines.push_back(line);
		}
	}

	// 关闭文件
	fclose(file);

	return true;
}

/**
 * @brief 初始化data
 */
void GridMap::initData()
{
	m_gridData = new DistLine *[m_width];
	for (int i = 0; i < m_width; ++i) {
		m_gridData[i] = new DistLine[m_height];
		for (int j = 0; j < m_height; ++j) {
			computeMinDist(i, j);
		}
	}
}

/**
 * @brief 计算距离最近的直线及相应的距离
 *
 * @param x 点的x坐标（厘米）
 * @param y 点的y坐标（厘米）
 */
void GridMap::computeMinDist(int x, int y)
{
	int minId = 0;
	float minDist = std::numeric_limits<float>::max(), dist;
	DistLine *line;

	for (std::vector<Line>::size_type i = 0; i < m_lines.size(); ++i) {
		dist = distToLine(x, y, m_lines[i]);
		if (dist < minDist) {
			minId = i;
			minDist = dist;
		}
	}

	line = &(m_gridData[x][y]); 
	line->id = minId;
	line->dist = minDist;
}
